//////////////////////////////////////////////////////////////////////////////
// Copyright (C) 2008-2010, Shane J. M. Liesegang
// All rights reserved.
// 
// Redistribution and use in source and binary forms, with or without 
// modification, are permitted provided that the following conditions are met:
// 
//     * Redistributions of source code must retain the above copyright 
//       notice, this list of conditions and the following disclaimer.
//     * Redistributions in binary form must reproduce the above copyright 
//       notice, this list of conditions and the following disclaimer in the 
//       documentation and/or other materials provided with the distribution.
//     * Neither the name of the copyright holder nor the names of any 
//       contributors may be used to endorse or promote products derived from 
//       this software without specific prior written permission.
// 
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
// POSSIBILITY OF SUCH DAMAGE.
//////////////////////////////////////////////////////////////////////////////

#include "PathFinder.h"
#include "SpatialGraph.h"
#include "TextRendering.h"
#include "DrawUtil.h"
#include "StringUtil.h"
#include "MathUtil.h"

bool PathFinder::_drawPaths = false;

PathFinder::PathFinder()
	: _currentPathIndex(-1),
	_currentState(PFMS_START),
	_arrivalDist(0.0f)
{
	InitializeStates();
}

PathFinder::~PathFinder()
{
	for(int i = 0; i < PFMS_COUNT; i++)
		delete _states[i];
}

void PathFinder::FindNextMove(const Vector2& from, const Vector2& to, float arrivalDist, PathFinderMove& move)
{
	_currentPos = from;
	_currentDest = to;
	_arrivalDist = arrivalDist;

	move.LastResult = PathFinder::PFMR_PATH_FOUND;

	while(!GetCurrentState()->Update(move));
}

void PathFinder::Render()
{
	if(!_drawPaths)
		return;
	if(_currentPath.size() > 0)
	{
		Vector2 lastPt;

		glColor3f(0, 0, 1.f);
		for(unsigned int i = 0; i <  _currentPath.size(); i++)
		{
			Vector2 curPt = _currentPath[i];
			if(lastPt != Vector2(0))
				DrawLine(curPt, lastPt);
			lastPt = curPt;
		}

		if(_currentPathIndex >= 0 && _currentPathIndex < (int)_currentPath.size())
		{
			glColor3f(1, 0, 1.f);
			DrawLine(_currentPath[_currentPathIndex], _currentPos);
			DrawPoint(_currentPath[_currentPathIndex], 0.1f);
		}
		DrawPoint(_currentPath[_currentPath.size() - 1], 0.1f);
	}
	String currentState = GetCurrentState()->GetName();

	Vector2 screenCenter = MathUtil::WorldToScreen(_currentPos.X, _currentPos.Y);
	// Print some vals
	glColor3f(0, 1.f, 1.f);
	DrawGameText(currentState + String("") + IntToString(_currentPathIndex), "ConsoleSmall", (int)screenCenter.X, (int)screenCenter.Y);

}

void PathFinder::EnableDrawPaths(bool enable)
{
	_drawPaths = enable;
}

class StartMoveState : public FindNextMoveState
{
public:
	virtual ~StartMoveState() {}
	virtual const char* GetName() { return "Start";}
	virtual bool Update(PathFinderMove& move)
	{
		if(!theSpatialGraph.IsInPathableSpace(GetCurrentPosition()))
		{
			SetNewState(PathFinder::PFMS_STARTRECOVER);
			return true;
		}
		// find path
		GetCurrentPath().clear();
		bool retVal = theSpatialGraph.GetPath(GetCurrentPosition(), GetCurrentDestination(), GetCurrentPath());

		if(retVal)
		{
			move.LastResult = PathFinder::PFMR_PATH_FOUND;
			// Initialize path index
			GetCurrentPathIndex() = 0;

			SetNewState(PathFinder::PFMS_FOLLOW);
			// keep ticking
			return true;
		}
		else
			move.LastResult = PathFinder::PFMR_PATH_NOT_FOUND;
		// We're done if path failed
		return false;
	}
};

class ValidateMoveState : public FindNextMoveState
{
public:
	virtual ~ValidateMoveState() {}
	virtual const char* GetName() {return "Validate";}
    virtual bool Update( PathFinderMove& /*move*/ )
	{
		// Make sure we can keep heading to our current path
		const Vector2List& currentPath = GetCurrentPath();
		int currentPathIndex = GetCurrentPathIndex();

		// has the destination changed
		Vector2 vDest = currentPath[currentPath.size() - 1];
		if(vDest == GetCurrentDestination())
		{
			if( theSpatialGraph.CanGo( GetCurrentPosition(), currentPath[currentPathIndex] ) )
			{
				SetNewState(PathFinder::PFMS_FOLLOW);
				return true;
			}
			else if(!theSpatialGraph.CanGo(GetCurrentPosition(), GetCurrentPosition() ) )
			{
				// are we blocked
				SetNewState(PathFinder::PFMS_RECOVER);
				return true;
			}
		}

		// otherwise, try again
		SetNewState(PathFinder::PFMS_RECOVER);
		return true;
	}

};

class FollowMoveState : public FindNextMoveState
{
public:
        virtual ~FollowMoveState() {}
        virtual const char* GetName() {return "Follow";}
        virtual bool Update( PathFinderMove& move )
        {
                //check our current path index
                const int iLookAheadCount = 3;

                const Vector2List& currentPath = GetCurrentPath();
                int nextPathIndex = GetCurrentPathIndex();

                //Are we at our goal index
                if( nextPathIndex == (int)currentPath.size() - 1 )
                {
                        move.NextSubgoalPos = currentPath[nextPathIndex];
                        //check distance to goal
                        float sqDist = Vector2::DistanceSquared( GetCurrentPosition(), move.NextSubgoalPos );
                        float arrivalDistSq = GetCurrentArrivalDist();
                        arrivalDistSq *= arrivalDistSq;
                        if( sqDist <= arrivalDistSq )
                        {
                                //don't set move dir (we've arrived)
                                move.LastResult = PathFinder::PFMR_ARRIVED;
                                SetNewState( PathFinder::PFMS_VALIDATE );
                                return false;
                        }
                }
                else
                {
                        //otherwise, see if we can advance our next subgoal
                        for( int i = 0 ; i < iLookAheadCount && (nextPathIndex+1) < (int)currentPath.size(); i++ )
                        {
                                if( theSpatialGraph.CanGo( GetCurrentPosition(), currentPath[nextPathIndex+1] ) )
                                {
                                        ++nextPathIndex;
                                }
                        }

                        GetCurrentPathIndex() = nextPathIndex;
                        move.NextSubgoalPos = currentPath[nextPathIndex];
                }

                //Move dir is normalized towards next subgoal
                move.MoveDir = Vector2::Normalize( move.NextSubgoalPos - GetCurrentPosition() );
                //we're done this round
                SetNewState( PathFinder::PFMS_VALIDATE );
                return false;
        }

};

class RecoverMoveState : public FindNextMoveState
{
public:
        virtual ~RecoverMoveState() {}
        virtual const char* GetName() {return "Recover";}
        virtual bool Update( PathFinderMove& move )
        {
                //are we back in pathable space?
                if( theSpatialGraph.IsInPathableSpace( GetCurrentPosition() ) )
                {
                        SetNewState( PathFinder::PFMS_FOLLOW );
                        return true;
                }

                const Vector2List& currentPath = GetCurrentPath();
                int currentPathIndex = GetCurrentPathIndex();

                //otherwise, head toward our current pathnode
                move.MoveDir = Vector2::Normalize( currentPath[currentPathIndex] - GetCurrentPosition() );

                return false;
        }
};

class StartRecoverMoveState : public FindNextMoveState
{
public:
        virtual ~StartRecoverMoveState() {}
        virtual const char* GetName() {return "StartRecover";}
        virtual bool Update( PathFinderMove& move )
        {
                //are we back in pathable space?
                if( theSpatialGraph.IsInPathableSpace( GetCurrentPosition() ) )
                {
                        SetNewState( PathFinder::PFMS_START );
                        return true;
                }

                //find the nearest non-blocked neighbor I can move to
                Vector2 vGoTo;
                if( theSpatialGraph.FindNearestNonBlocked( GetCurrentPosition(), vGoTo) )
                {
                        move.MoveDir = Vector2::Normalize( vGoTo - GetCurrentPosition() );
                }
                else
                {
                        move.LastResult = PathFinder::PFMR_PATH_NOT_FOUND;
                }

                return false;
        }
};

void PathFinder::InitializeStates()
{
        //Init states
        _states[PFMS_START] = new StartMoveState();
        _states[PFMS_START]->Initialize( this );

        _states[PFMS_VALIDATE] = new ValidateMoveState();
        _states[PFMS_VALIDATE]->Initialize( this );

        _states[PFMS_FOLLOW] = new FollowMoveState();
        _states[PFMS_FOLLOW]->Initialize( this );

        _states[PFMS_RECOVER] = new RecoverMoveState();
        _states[PFMS_RECOVER]->Initialize( this );

        _states[PFMS_STARTRECOVER] = new StartRecoverMoveState();
        _states[PFMS_STARTRECOVER]->Initialize( this );


}


FindNextMoveState* PathFinder::GetCurrentState()
{
        return _states[_currentState];
}
void PathFinder::SetNewState( ePFMoveState newState )
{
        //ignore gotos to the same state
        if( newState == _currentState )
                return;

        if( newState < 0 || newState >= PFMS_COUNT )
                return;

        _currentState = newState;
}


void FindNextMoveState::SetNewState( PathFinder::ePFMoveState newState )
{
        _pathFinder->SetNewState( newState );
}

